import { Loading, Message } from 'element-ui';
import store from '../store/index'
import { ICON } from './icon';
import { subButtery, showStationDetail } from './methods'
'use strict';
export let ros, viewer;

var OBJ = new DataServer();

function DataServer() {
    // TODO
};
//继承DATA属性，获取：DATA.DataServer.
DataServer.prototype.register = function (key, value, options) {
    if (this.hasOwnProperty(key)) {
        return;
    }
    var fnGetDefault = function () {
        return value;
    };
    var fnSetDefault = function (val) {
        if (val === value) {
            return;
        }
        if (val.hasOwnProperty('property')) {
            value = val.value;
        }
        else {
            value = val;
        }
    };
    var fnGet = options.fnGet || fnGetDefault;
    var fnSetOptions = options.fnSet || function () { };
    var fnSet = function (val) {
        fnSetDefault(val);
        fnSetOptions(val);
    };
    //Object.defineProperty：添加或修改属性
    //对象，属性名字，属性特征
    Object.defineProperty(this, key, {
        get: fnGet,
        set: fnSet
    });
};

DataServer.prototype.syncToLocalStorage = function () {
    //TODO
};

function register() {
    return new Promise(function (resolve, reject) {
        OBJ.register('topic', {}, {});
        OBJ.register('tfMsg', {}, {});
        OBJ.register('mapInfo', null, {});
        OBJ.register('mapStage', null, {});
        OBJ.register('robot', null, {});
        OBJ.register('laserScan', null, {});
        OBJ.register('laserScanMsg', null, {});
        OBJ.register('laserScan', null, {});
        OBJ.register('wpContainer', null, {});
        OBJ.register('waypointsStage', null, {});
        OBJ.register('robotFootprint', null, {});
        resolve('register OBJ');
    })
}

//初始化ros
function rosInit() {
    return new Promise(function (resolve, reject) {
        // var url = window.location.hostname;
        var url = '10.42.0.1';
        ros = new ROSLIB.Ros({
            url: 'ws://' + url + ':9090',
            groovyCompatibility: true
        });

        //链接到websocket服务器
        ros.on('connection', () => {
            console.log(`[INFO]Connected to rosbridge ${url}.`);
            resolve('ros Connected');
        });
        //关闭websocket服务器
        ros.on('close', () => {
            console.log(`[INFO]Rosbridge server ${url} closed.`);
            Message({
                showClose: true,
                message: 'Rosbridge连接已关闭',
                type: 'error',
                duration: 0
            });
            reject('Rosbridge连接已关闭')
        });
        //关闭websocket服务器出错
        ros.on('error', () => {
            Message({
                showClose: true,
                message: 'Rosbridge连接失败',
                type: 'error',
                duration: 0
            });
            reject('Rosbridge连接失败')
            console.error(`[ERROR]Connection error.`);
        });
    })

}
/**
 * listenTF
 * map: 地图坐标，固定坐标系
 * odom: 机器人初始坐标，固定坐标系
 * base_link：机器人中心位置坐标
 * **/
function listenTf() {
    return new Promise(function (resolve, reject) {
        //从ros中订阅TFS
        var tfClient = new ROSLIB.TFClient({
            ros: ros,
            fixedFrame: 'map',
            angularThres: 0.01,
            transThres: 0.01
        });
        tfClient.subscribe('odom', (tf) => {
            OBJ.tfMsg['map2odom'] = {
                header: {
                    stamp: null
                },
                transform: tf
            };
        });
        tfClient.subscribe('base_footprint', (tf) => {
            OBJ.tfMsg['map2base_footprint'] = {
                header: {
                    stamp: null
                },
                transform: tf
            };
        });
        tfClient.subscribe('base_laser', (tf) => {
            OBJ.tfMsg['map2base_laser'] = {
                header: {
                    stamp: null
                },
                transform: tf
            };
        });
        var tfClient2 = new ROSLIB.TFClient({
            ros: ros,
            fixedFrame: 'base_link',
            angularThres: 0.01,
            transThres: 0.01
        });
        tfClient2.subscribe('base_laser', (tf) => {
            OBJ.tfMsg['base_link2base_laser'] = {
                header: {
                    stamp: null
                },
                transform: tf
            };
        });
        resolve('tf success');
    })
}
//initStage
function initStage() {
    return new Promise(function (resolve, reject) {
        var screenWidth = window.innerWidth || document.body.clientWidth || document.documentElement.clientWidth;
        var screenHeight = window.innerHeight || document.body.clientHeight || document.documentElement.clientHeight;
        var rMap = screenWidth / screenHeight;//比例
        var width = screenWidth;
        var height = screenHeight;
        if (screenWidth > screenHeight) {
            width = screenHeight * rMap;
            height = screenHeight;
        } else {
            width = screenWidth;
            height = screenWidth / rMap;
        }
        OBJ.mapInfo = {
            windowWidth: width,
            windowHeight: height
        };

        // Create the main viewer.
        viewer = new ROS2D.Viewer({
            divID: 'mapNavDiv',
            width: OBJ.mapInfo.windowWidth,
            height: OBJ.mapInfo.windowHeight
        });

        var zoomview = new ROS2D.ZoomView({
            rootObject: viewer.scene
        });

        //实例化画布对象
        var stage = viewer.scene;

        // mousewheel鼠标方法缩小
        event_util.addHandler(document, "mousewheel", function (event) {
            event = event_util.getEvent(event);
            if (event.layerX > 600 || event.layerY > 600) {
                return false;
            }
            if (parseFloat(event.wheelDelta) > 0) {
                zoomview.startZoom(event.layerX, event.layerY);
                zoomview.zoom(1.2);
            } else {
                zoomview.startZoom(event.layerX, event.layerY);
                zoomview.zoom(0.8);
            }
        });
        createjs.Touch.enable(stage);//开启触摸，disable禁止触摸
        createjs.Ticker.setFPS(25);//一秒25帧

        /**
         * 拖拽移动画布
         * **/
        stage.enableMouseOver(10);//用到mouseover要加上这一句


        createjs.Touch.enable(stage);//移动端也支持点击移动事件，允许设备触控

        stage.mouseMoveOutside = true;//鼠标离开画布继续调用鼠标移动事件

        stage.on("mousedown", function (evt) {
            if (store.state.disabledMoveMap) {
                return
            } else {
                // keep a record on the offset between the mouse position and the container position. currentTarget will be the container that the event listener was added to:
                evt.currentTarget.offset = { x: this.x - evt.stageX, y: this.y - evt.stageY };
            }
        });
        stage.on("pressmove", function (evt) {
            if (store.state.disabledMoveMap) {
                return
            } else {
                // Calculate the new X and Y based on the mouse new position plus the offset.
                evt.currentTarget.x = evt.stageX + evt.currentTarget.offset.x;
                evt.currentTarget.y = evt.stageY + evt.currentTarget.offset.y;
                // make sure to redraw the stage to show the change:
                stage.update();
            }
        });
        stage.update();

        //刻度事件，实时监听变化
        createjs.Ticker.addEventListener('tick', function () {
            //更新，重新绘制画布
            stage.update();
        });
        resolve('initStage success')
    })
}

//init map topic
function mapTopic() {
    return new Promise(function (resolve, reject) {
        //map topic
        var mapTopic = new ROSLIB.Topic({
            ros: ros,
            name: '/map_stream',
            messageType: 'scheduling_msgs/MapStream'
        });
        OBJ.topic['mapTopic'] = mapTopic;

        //
        resolve('init topics')
    })
}

//显示/隐藏地图
export function showMap(boole) {
    return new Promise(function (resolve, reject) {
        var mapTopic = OBJ.topic.mapTopic;
        if (boole) {
            if (OBJ.mapStage){
                viewer.scene.removeChild(OBJ.mapStage);
            }
            var currentImage = new createjs.Shape();
            mapTopic.subscribe((message) => {
                // create the image
                currentImage = new ROS2D.ImageMap({
                    message: message.info,
                    image: message.data,
                })
                OBJ.mapInfo.mapMsg = message;

                var rMap = message.info.width / message.info.height;
                var win_width = OBJ.mapInfo.windowWidth;
                var win_height = OBJ.mapInfo.windowHeight;
                if (win_width > win_height) {
                    win_width = win_height * rMap;
                    win_height = win_height;
                } else {
                    win_width = win_width;
                    win_height = win_width / rMap;
                }
                var scale = {
                    x: win_width / OBJ.mapInfo.windowWidth || 1.0,
                    y: win_height / OBJ.mapInfo.windowHeight || 1.0
                }

                //修改map坐标和缩放
                currentImage.scaleX = OBJ.mapInfo.windowWidth / message.info.width * scale.x;
                currentImage.scaleY = OBJ.mapInfo.windowHeight / message.info.height * scale.y;
                currentImage.regX = (win_width - OBJ.mapInfo.windowWidth) / 2 || 0;
                currentImage.regY = (win_height - OBJ.mapInfo.windowHeight) / 2 || 0;
                currentImage.x = 0;
                currentImage.y = 0;
                createjs.stage = currentImage;
                // //地图数据，缩放，xy等
                OBJ.mapStage = currentImage;
                viewer.scene.y = 0;
                viewer.scene.addChild(currentImage);
                // work-around for a bug in easeljs -- needs a second object to render correctly
                viewer.scene.addChild(new ROS2D.Grid({ size: 1 }));
                resolve('map init success')
            });
            viewer.scene.addChild(OBJ.mapStage);
        } else {
            viewer.scene.removeChild(OBJ.mapStage);
        }
        resolve()
    })
}

//init robot topic
function robotTopic() {
    return new Promise(function (resolve, reject) {
        var robotPoseTopic = new ROSLIB.Topic({
            ros: ros,
            name: '/amcl_pose',
            messageType: 'geometry_msgs/PoseWithCovarianceStamped',
            throttle_rate: 100
        });
        OBJ.topic['robotPoseTopic'] = robotPoseTopic;
        resolve('robot topic')
    })
}

//show robot
export function showRobot(boole) {
    return new Promise(function (resolve, reject) {
        var robotPoseTopic = OBJ.topic.robotPoseTopic;
        if (boole) {
            robotPoseTopic.subscribe((message) => {
                if (!OBJ.robot) {
                    OBJ.robot = ICON.robot({});
                    viewer.scene.addChild(OBJ.robot);
                } else {
                    viewer.scene.addChild(OBJ.robot);
                }
                try {
                    // var pos = viewer.scene.rosToGlobal(message.pose.pose.position)
                    var pos = rosToPx({
                        x: message.pose.pose.position.x,
                        y: message.pose.pose.position.y
                    });
                    OBJ.robot.x = pos.x;
                    OBJ.robot.y = pos.y;
                    // change the angle
                    OBJ.robot.rotation = viewer.scene.rosQuaternionToGlobalTheta(message.pose.pose.orientation);
                    // Set visible
                } catch (err) {
                    console.warn(err)
                }
                resolve('robot init success')
            });
        } else {
            robotPoseTopic.unsubscribe();
            viewer.scene.removeChild(OBJ.robot);
        }
        resolve()
    })
}

//轮廓 topic
function footprintTopic() {
    return new Promise(function (resolve, reject) {
        var footprintTopic = new ROSLIB.Topic({
            ros: ros,
            name: '/move_base/global_costmap/footprint',
            messageType: 'geometry_msgs/PolygonStamped'
        });
        OBJ.topic['footprintTopic'] = footprintTopic;
        resolve('轮廓 topic')
    })
}

//show 轮廓
export function showFootprint(boole) {
    return new Promise(function (resolve, reject) {
        var footprintTopic = OBJ.topic.footprintTopic;
        if (boole) {
            footprintTopic.subscribe((message) => {
                if (OBJ.robotFootprint) {
                    viewer.scene.removeChild(OBJ.robotFootprint)
                }
                var footprintStage = ICON.footprint(message, {
                    strokeColor: '#8bc34a'
                });
                OBJ.robotFootprint = footprintStage;
                viewer.scene.addChild(footprintStage)
            });
        } else {
            footprintTopic.unsubscribe();
            viewer.scene.removeChild(OBJ.robotFootprint);
        }
        resolve('轮廓success')
    })
}
export var rosToPx = (pos) => {
    if (!OBJ.mapStage) {
        console.warn('[WARN]map not available');
        return;
    }
    var mapPosition = OBJ.mapInfo.mapMsg.info.origin.position;
    var mapResolution = OBJ.mapInfo.mapMsg.info.resolution;
    var height = OBJ.mapStage.image.height;
    var x = Math.round((pos.x - mapPosition.x) / mapResolution
        * OBJ.mapStage.scaleX);
    var y = Math.round((height - (pos.y - mapPosition.y) / mapResolution)
        * OBJ.mapStage.scaleY);
    return {
        x: x - OBJ.mapStage.regX * OBJ.mapStage.scaleX,
        y: y - OBJ.mapStage.regY * OBJ.mapStage.scaleY
    };
}

//判断四元数是否有效
var isTfValid = (tfStamp, msgStamp, duration) => {
    if (!tfStamp) {
        return true;
    }
    var secs = Math.abs(tfStamp.secs - msgStamp.secs);
    var nsecs = Math.abs(tfStamp.nsecs - msgStamp.nsecs);
    var t = secs + nsecs / Math.pow(10, 9);
    return (t < duration);
}

//获取姿态参数
var getTfMat = (transform) => {
    var translation = transform.translation;
    var yaw = quaternionToYaw(transform.rotation);
    var sinYaw = Math.sin(yaw);
    var cosYaw = Math.cos(yaw);
    var tx = translation.x;
    var ty = translation.y;
    return $M([[cosYaw, -sinYaw, tx],
    [sinYaw, cosYaw, ty],
    [0, 0, 1]]);
}

//激光测距上下
var isLaserUpsideDown = (rotation) => {
    //四元数转换度数
    var roll = quaternionToRoll(rotation);
    //Math.PI:180度
    if (Math.abs(roll - Math.PI) < 0.01) {
        return 1;
    }
    else {
        return 0;
    }
}

//四元数转换为旋转角度
var quaternionToRoll = (orientation) => {
    var rotation = orientation;
    var numerator = 2 * (rotation.w * rotation.x + rotation.y * rotation.z);
    var denominator = 1 - 2 * (Math.pow(rotation.x, 2) + Math.pow(rotation.y, 2));
    var yaw = Math.atan2(numerator, denominator);
    return yaw;
}

var quaternionToYaw = (orientation, ignoreXY) => {
    var rotation = orientation;
    var numerator;
    var denominator;
    if (ignoreXY) {
        numerator = 2 * rotation.w * rotation.z;
        denominator = 1 - 2 * Math.pow(rotation.z, 2);
    } else {
        numerator = 2 * (rotation.w * rotation.z + rotation.x * rotation.y);
        denominator = 1 - 2 * (Math.pow(rotation.y, 2) + Math.pow(rotation.z, 2));
    }
    var yaw = Math.atan2(numerator, denominator);
    return yaw;
};
var laserScanBase_laserToMap = (laserScanMsg) => {
    var skipNum = 10;
    var laserScanMsgMap = [];
    if (OBJ.tfMsg.hasOwnProperty('map2base_laser')) {
        var tfs = [OBJ.tfMsg.map2base_laser];
    } else {
        var tfs = [OBJ.tfMsg.map2odom, OBJ.tfMsg.odom2base_footprint,
        OBJ.tfMsg.base_footprint2base_link, OBJ.tfMsg.base_link2base_laser];
    }
    if (OBJ.laserScanMsg.header.frame_id === 'base_footprint') {
        if (OBJ.tfMsg.hasOwnProperty('map2base_footprint')) {
            tfs = [OBJ.tfMsg.map2base_footprint];
        } else {
            tfs = [OBJ.tfMsg.map2odom, OBJ.tfMsg.odom2base_footprint];
        }
    }
    var isTfAvailable = true;
    for (var i = 0; i < tfs.length; i++) {
        if (!tfs[i]) {
            isTfAvailable = false;
            break;
        }
    }
    if (!isTfAvailable) {
        console.warn('[WARN]One or more Tf msg from map to base_laser not available');
        return laserScanMsgMap;
    }
    var isValid = true;
    for (var i = 0; i < tfs.length; i++) {
        isValid = isValid && isTfValid(tfs[i].header.stamp, laserScanMsg.header.stamp, 2.0);
    }
    if (!isValid) {
        console.warn('[WARN]One or more Tf msg from map to base_laser out of date');
        return laserScanMsgMap;
    }
    var tfMat = $M([[1, 0, 0],
    [0, 1, 0],
    [0, 0, 1]]);
    for (var i = 0; i < tfs.length; i++) {
        tfMat = tfMat.multiply(getTfMat(tfs[i].transform));
    }
    // check if laser upside down
    if (OBJ.tfMsg.base_link2base_laser) {
        var rotation = OBJ.tfMsg.base_link2base_laser.transform.rotation;
        var isUpsideDown = isLaserUpsideDown(rotation);
    } else {
        // TODO: replace this
        var isUpsideDown = 0;
    }
    for (var i = 0; i < laserScanMsg.ranges.length; i += skipNum) {
        if (laserScanMsg.ranges[i] === 'inf' || laserScanMsg.ranges[i] < laserScanMsg.range_min
            || laserScanMsg.ranges[i] > laserScanMsg.range_max) {
            continue;
        }
        var angle = laserScanMsg.angle_min + laserScanMsg.angle_increment * i;
        var pos = [
            laserScanMsg.ranges[i] * Math.cos(angle),
            laserScanMsg.ranges[i] * Math.sin(angle) * Math.pow(-1, isUpsideDown),
            1
        ];
        var posVec = $V(pos);
        var result = tfMat.multiply(posVec);
        laserScanMsgMap.push({
            x: result.elements[0],
            y: result.elements[1]
        });
    }
    return laserScanMsgMap;
}



/**
 * 显示站点
 */
function showWayPoints() {
    return new Promise(function (resolve, reject) {
        var wpTopic = new ROSLIB.Topic({
            ros: ros,
            name: '/waypoints',
            messageType: 'yocs_msgs/WaypointList'
        });
        wpTopic.subscribe((message) => {
            OBJ.wayPointsMsg = message;
            var waypointContainers = {
                goal: [],
            };
            //获取到站点信息之后赋给waypointsMsg，渲染站点，添加事件
            for (var i = 0; i < message.waypoints.length; i++) {
                var waypoint = message.waypoints[i];
                var wpInfo = JSON.parse(waypoint.header.frame_id);
                var waypointType = wpInfo.type;
                //显示在地图中
                var wpXY = rosToPx(waypoint.pose.position);
                //如果不是目标站点，直接跳过
                if (waypointType !== 'goal') {
                    continue;
                }
                var wpContainer = ICON[waypointType]({});
                viewer.scene.addChild(wpContainer)
                wpContainer.x = wpXY.x;
                wpContainer.y = wpXY.y;
                //icon旋转
                wpContainer.rotation = viewer.scene.rosQuaternionToGlobalTheta(waypoint.pose.orientation);
                wpContainer.name = waypoint.name; // bind name
                waypointContainers[waypointType].push(wpContainer);
                // TODO: handle wp click
                //点击站点导航到站点
                wpContainer.on('click', waypointClick);
            }
            OBJ.waypointsStage = waypointContainers;
            resolve('wayPoints success');
        });
    })
}

//点击站点事件
function waypointClick(event) {
    showStationDetail(event.currentTarget.name)
}



//laserScan topic
function laserScanTopic() {
    return new Promise(function (resolve, reject) {
        var laserScanTopic = new ROSLIB.Topic({
            ros: ros,
            name: '/scan_rectified',
            messageType: 'sensor_msgs/LaserScan',
            throttle_rate: 200
        });
        OBJ.topic['laserScanTopic'] = laserScanTopic;
        resolve('laserScanTopic')
    })
}
//显示/隐藏激光
export function showLaserScan(boole) {
    if (boole) {
        OBJ.topic.laserScanTopic.subscribe((message) => {
            OBJ.laserScanMsg = message;
            if (OBJ.laserScan) {
                viewer.scene.removeChild(OBJ.laserScan);
            }
            var laserScanPoints = laserScanBase_laserToMap(message);
            var laserScan = ICON.laserScan(laserScanPoints, {
                fillColor: '#ff9800'
            });
            OBJ.laserScan = laserScan;
            viewer.scene.addChild(laserScan)
        });
    } else {
        OBJ.topic.laserScanTopic.unsubscribe();
        viewer.scene.removeChild(OBJ.laserScan)
    }
}

/**
 * 显示全局路径
 * **/
function showGlobalPath() {
    try {
        return new Promise(function (resolve, reject) {
            var globalPlanTopic = new ROSLIB.Topic({
                ros: ros,
                name: '/move_base/GlobalPlanner/plan',
                messageType: 'nav_msgs/Path'
            });
            globalPlanTopic.subscribe((message) => {
                var globalPath = new ROS2D.PathShape({
                    path: message
                })
            });
            resolve()
        })
    } catch (err) {
        console.log(err)
    }

}

//首先进行加载
$(() => {
    Loading.service({
        text: '加载中...'
    })
    Promise.all([register(), rosInit(), initStage(), listenTf(), mapTopic(), showMap(true), robotTopic(), showRobot(true), footprintTopic(), showFootprint(true), showWayPoints(), laserScanTopic()])
        .then((result) => {
            console.log(result)
            Loading.service().close();
        })
        .catch((error) => {
            console.log(error)
            Loading.service().close();
        })
});

/**
 * 通过名称找到对应站点数据
 * **/
export function getWaypointByName(name) {
    for (var i = 0; i < OBJ.wayPointsMsg.waypoints.length; i++) {
        var waypoint = OBJ.wayPointsMsg.waypoints[i];
        if (waypoint.name === name) {
            return waypoint;
        }
    }
};